#include "zf_common_headfile.h"

#ifndef _PID_h_
#define _PID_h_

typedef struct {
    float kp;
    float ki;
    float kd;
    float integral;
    float prev_error;
    float output_limit;
    float dead_zone;
}PID_typedef;

extern PID_typedef Pid;
extern PID_typedef AnglePid;
extern PID_typedef RatePid;

void Pid_init(float p, float i, float d, float output_limit, float dead_zone);
void AnglePid_init(float p, float i, float d, float output_limit, float dead_zone);
void RatePid_init(float p, float i, float d, float output_limit, float dead_zone);

int PID_Control(float target, float current);
int AnglePID_Control(float target_angle, float current_angle);
int RatePID_Control(float target_rate, float current_rate);
void set_speed(int16 left_speed,int16 right_speed,int error1,int error2);

#endif
